/**
 * @file nd03_dev.c
 * @author tongsheng.tang
 * @brief ND03 device setting functions
 * @version 1.x.x
 * @date 2021-11
 *
 * @copyright Copyright (c) 2021, Shenzhen Nephotonics Inc.
 *
 */

#include <stdio.h>
#include "nd03_platform.h"
#include "nd03_comm.h"
#include "nd03_dev.h"
#include "nd03_data.h"
#include "nd03_calib.h"
#include "nd03_def.h"
#include "zino.h"
#include "fc_error.h"
#include "sensordata.h"

#define DBG_TAG "nd03"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
/** 模组设备 */
ND03_Dev_t g_nd03_device = {.i2c_dev_addr = ND03_DEFAULT_SLAVE_ADDR, .SetXShutPinLevelFunc = set_xshut_pin_level};

/**
 * @brief 单次测量接口
 *
 * @param pNxDevice 模组设备
 */
void SG_RangingTest(ND03_Dev_t *pNxDevice, laser_t *sensor)
{
    ND03_RangingData_t data;
    /* 请求一次测量 */
    ND03_StartMeasurement(pNxDevice);
    /* 获取测量数据 */
    ND03_GetRangingData(pNxDevice, &data);
    sensor->range = data.depth;
    sensor->quality = data.amp;
    NX_PRINTF("depth:%d mm, amp:%d, T:%d exp:%d\r\n", data.depth, data.amp, ND03_GetCurrentTherm(pNxDevice), ND03_GetCurrentExp(pNxDevice));
}

void cnt_ranging_get(ND03_Dev_t *pNxDevice, laser_t *sensor)
{
    ND03_RangingData_t data;
    ND03_ReadDepthAndAmpData(pNxDevice, &data);
    ND03_ClearDataValidFlag(pNxDevice);
    sensor->range = data.depth;
    sensor->quality = data.amp;
    NX_PRINTF("depth:%d mm, amp:%d, T:%d exp:%d\r\n", data.depth, data.amp, ND03_GetCurrentTherm(pNxDevice), ND03_GetCurrentExp(pNxDevice));
}

/**
 * @brief ND03 single ranging example main function
 *
 * @return int32_t
 */
int nd03_main(void *p)
{
    /* 函数指针结构体 */
    ND03_Func_Ptr_t dev_op = {NULL, NULL, NULL, NULL};

    /* 循环测距次数为100次 */
    int32_t cnt = 10000;

    /* 初始化函数指针结构体 */
    dev_op.Delay10usFunc = delay_10us;
    dev_op.Delay1msFunc = delay_1ms;
    dev_op.I2C_ReadNBytesFunc = i2c_read_nbytes;
    dev_op.I2C_WriteNBytesFunc = i2c_write_nbytes;

    dn03_platform_init();

    /* 将host端功能函数注册到SDK中 */
    ND03_RegisteredPlatformApi(dev_op);

    /* 初始化设备变量的IIC地址 */
    g_nd03_device.i2c_dev_addr = ND03_DEFAULT_SLAVE_ADDR;

    /* 循环等待设备启动, 若模组或者IIC读写函数有问题则会报错 */
    if (ND03_ERROR_NONE != ND03_WaitDeviceBootUp(&g_nd03_device))
    {
        NX_PRINTF("ND03_WaitDeviceBootUp error\r\n");
        FC_ERROR_SET(FC_LASER_HARDFAULT);
        return -1;
    }

    /* 初始化模组设备 */
    if (ND03_ERROR_NONE != ND03_InitDevice(&g_nd03_device))
    {
        NX_PRINTF("ND03_InitDevice error!!\r\n");
        FC_ERROR_SET(FC_LASER_HARDFAULT);
        return -1;
    }
    ND03_SetRangeMode(&g_nd03_device, ND03_DEVICEMODE_CONTINUOUS_RANGING|ND03_RANGE_MODE_FAST|ND03_CALCU_MODE_AVER);
    if (FC_ERROR_CHECK(FC_LASER_HARDFAULT))
        FC_ERROR_RESET(FC_LASER_HARDFAULT);

    /* 循环测量 */
    while (1)
    {
        // SG_RangingTest(&g_nd03_device, &sensorData.tof);
        cnt_ranging_get(&g_nd03_device, &sensorData.tof);
        rt_thread_mdelay(10);
    }

    return 0;
}
void nd03(int *argc, char **argv)
{
    if (rt_strcmp(argv[1], "calib") == 0)
    {
        if (argc == 3)
        {
            uint32_t depth = atoi(argv[2]);
            rt_kprintf("calib at depth:%d\r\n", depth);
            ND03_OffsetCalibrationAtDepth(&g_nd03_device, depth);
        }
        else
        {
            rt_kprintf("calib at default (150mm) depth\r\n");
            ND03_OffsetCalibration(&g_nd03_device);
        }
    }
    else
    {
        rt_kprintf("calib [depth]\r\n");
    }
}
MSH_CMD_EXPORT(nd03, nd03 cmd);
int nd03_task_init(void)
{

    rt_thread_t tid = rt_thread_create("ND03",
                                       nd03_main,
                                       RT_NULL,
                                       2048,
                                       20,
                                       10);
    if (tid != RT_NULL)
    {
        LOG_I("create thread success");
        rt_thread_startup(tid);
        return RT_EOK;
    }
    else
    {
        LOG_E("create thread error");
        return -RT_ENOMEM;
    }
    return RT_EOK;
}

ZINO_APP_EXPORT(nd03_task_init);
